﻿using System;
using System.Collections.Generic;
using System.Linq;
using Algoritmia.Utils;

namespace Algoritmia.DataStructures.DoubleEndedPriorityQueues
{
    public class IntervalHeap<T> : 
        IDoubleEndedPriorityQueue<T>,
        IEnumerable<T>
    {
        private T[] _heap;
        private readonly EqualityComparison<T>_firstHasHigherPriority;
        private readonly EqualityComparison<T> _firstHasLowerPriority;
        private int _capacity;
        private int _count;

        public IntervalHeap(IEnumerable<T> items=null, int capacity = 0, 
            EqualityComparison<T> firstHasHigherPriority = null)
        {
            items = items ?? Enumerable.Empty<T>();
            _heap = items.ToArray();
            _count = _heap.Length;
            if (capacity <= 0) capacity = 1;
            if ( capacity > _heap.Length)
            {
                _capacity = capacity;
                Array.Resize(ref _heap,_capacity);
            }
            else
            {
                _capacity = _heap.Length;
            }
            _firstHasHigherPriority = GetFirstHasHigherPriority(firstHasHigherPriority);
            _firstHasLowerPriority =  ((value1,value2) => _firstHasHigherPriority(value2,value1));

            for(int i = 0; i < _count; i+=2)
            {
                if (i + 1 < _count)
                {
                    SwapBrothers(i);
                }
            }

            int lastParent = Parent(_count - 1);

            for(int i = lastParent; i >= 0 ; i-=2)
            {
                HeapifyHigher(i);
                HeapifyLower(i);
            }
        }

        private static EqualityComparison<T> GetFirstHasHigherPriority(EqualityComparison<T> firstHasHigherPriority)
        {
            if (firstHasHigherPriority == null)
            {
                if ( typeof(T).GetInterfaces().Contains(typeof(IComparable<T>)))
                {
                    return (v1,v2) => Comparer<T>.Default.Compare(v1, v2) < 0;
                }
                throw new ArgumentException("No comparison criterium on non comparable data.");
            }
            return firstHasHigherPriority;
        }

        private void HeapifyLower(int index)
        {
            int lowest = index+1;
            while (true)
            {
                foreach (int child in Children(index))
                {
                    if (child + 1 < _count)
                    {
                        if (_firstHasLowerPriority(_heap[child + 1], _heap[lowest]))
                        {
                            lowest = child+1;
                        }
                    }
                    else
                    {
                        if (_firstHasLowerPriority(_heap[child],_heap[lowest]))
                        {
                            lowest = child;
                        }
                    }
                }
                if (lowest == index+1)
                {
                    break;
                }
                Swap(index+1, lowest);
                index = lowest / 2 * 2;
                if (index + 1 < _count)
                {
                    SwapBrothers(index);
                }
                else
                {
                    break;
                }
            }
        }

        private void HeapifyHigher(int index)
        {
            int highest = index;
            while(true)
            {
                foreach(int child in Children(index))
                {
                    if (_firstHasHigherPriority(_heap[child],_heap[highest]) )
                    {
                        highest = child;
                    }
                }
                if (highest == index) break;

                Swap(index,highest);
                index = highest;
                if (index + 1 < _count)
                {
                    SwapBrothers(index);
                }
            }

           
        }

        private void BubbleUpHigher(int index)
        {
            while(true)
            {
                int parent = Parent(index);
                if (_firstHasHigherPriority(_heap[index], _heap[parent]))
                {
                    Swap(index, parent);
                    index = parent;
                }
                else break;
            }
        }

        private void BubbleUpLower(int index)
        {
            while (true)
            {
                int parent = Parent(index);
                if (_firstHasLowerPriority(_heap[index + 1], _heap[parent + 1]))
                {
                    Swap(index + 1, parent + 1);
                    index = parent;
                }
                else break;
            }
        }

        private int Parent(int son)
        {
            return ((son/2 - 1)/2)*2;
        }

        private IEnumerable<int> Children(int parent)
        {
            int j = 2*(parent + 1);
            if (j < _count)
                yield return j;
            j += 2;
            if (j < _count)
                yield return j;
        }

        private void Swap(int index1,int index2)
        {
            T aux = _heap[index2];
            _heap[index2] = _heap[index1];
            _heap[index1] = aux;
        }

        private bool SwapBrothers(int index)
        {
            if (_firstHasHigherPriority(_heap[index+1],_heap[index]))
            {
                Swap(index,index+1);
                return true;
            }
            return false;
        }

        #region IDoubleEndedPriorityQueue<T> Members

        public void Add(T elem)
        {
            if ( _count == _capacity)
            {
                _capacity *= 2;
                Array.Resize(ref _heap,_capacity);
            }
            if ( _count == 0)
            {
                _heap[0] = elem;
                _count++;
                return;
            }
            _heap[_count++] = elem;
            if ( _count % 2 == 0)
            {
                int brother = _count - 2;
                if (SwapBrothers(brother))
                {
                    BubbleUpHigher(brother);
                }
                else
                {
                    BubbleUpLower(brother);
                }
            }
            else
            {
                int index = _count - 1;
                int parent = Parent(index);
                if (parent >= 0)
                {
                    if ( _firstHasHigherPriority(_heap[parent],elem) &&
                        _firstHasLowerPriority(_heap[parent+1],elem))
                    {
                        return;
                    }
                    if (_firstHasHigherPriority(elem,_heap[parent]))
                    {
                        Swap(index,parent);
                        BubbleUpHigher(parent);
                    }
                    else if (_firstHasLowerPriority(elem,_heap[parent+1]))
                    {
                        
                        Swap(index,parent+1);
                        BubbleUpLower(parent);
                    }
                }
            }
        }

        public T ExtractOptimal()
        {
            if (_count == 0)
            {
                throw new InvalidOperationException("Can't extract anything from an empty interval heap.");
            }
            if (_count == 1)
            {
                _count--;
                return _heap[0];
            }
            if (_count == 2)
            {
                Swap(1,0);
                _count--;
                return _heap[1];
            }
            T value = _heap[0];
            Swap(0, _count - 1);
            _count--;
            HeapifyHigher(0);
            return value;
        }

        public T GetOptimal()
        {
            if ( _count == 0)
            {
                throw new InvalidOperationException("Can't get anything from an empty interval heap.");
            }
            return _heap[0];
        }

        public T GetWorst()
        {
            if ( _count == 0)
            {
                throw new InvalidOperationException("Can't get anything from an empty interval heap.");
            }
            if ( _count == 1)
            {
                return _heap[0];
            }
            return _heap[1];
        }

        public T ExtractWorst()
        {
            if (_count == 0)
            {
                throw new InvalidOperationException("Can't extract anything from an empty interval heap.");
            }
            if (_count == 1)
            {
                _count--;
                return _heap[0];
            }
            if ( _count == 2)
            {
                _count--;
                return _heap[1];
            }
            T value = _heap[1];
            Swap(1,_count-1);
            _count--;
            HeapifyLower(0);
            return value;
        }

        #endregion

        #region ICountable Members

        public int Count
        {
            get { return _count; }
        }

        #endregion

        #region IEnumerable<T> Members

        public IEnumerator<T> GetEnumerator()
        {
            for (int i = 0; i < _count; i++)
                yield return _heap[i];
        }

        #endregion

        #region IEnumerable Members

        System.Collections.IEnumerator System.Collections.IEnumerable.GetEnumerator()
        {
            return GetEnumerator();
        }

        #endregion

        public override string ToString()
        {
            return String.Format("{0} <{1}> [{2}]", GetType().Name, typeof(T).Name,string.Join(",", this));
        }
    }
}
